﻿
// 2024/2/23: 首个版本
// 2024/7/3: 基于新样本框架
// 2025/8/4: 不使用cat宏
// 2025/8/4: 改为继承SessionSample。改回单头文件

#ifndef VEHICLE_CONTROL_V1_H
#define VEHICLE_CONTROL_V1_H

#include "spadas.h"

namespace vehicle_control_v1
{
	using namespace spadas;

	enum class LongitudinalControlMode
	{
		NoRequest = 0, // Not requesting longitudinal control / 不请求纵向控制
		Position = 1, // Control throttle and brake pedal, value ranges from -1(max brake) ~ 1(max throttle) / 控制油门、刹车踏板，数值范围为-1(刹车到底)~1(油门到底)
		Acceleration = 2, // Control acceleration and deceleration, in m/s² units (The target absolute speed is necessary to set) / 控制加、减速度，数值单位为m/s²，同时需要指定目标绝对速度
	};

	enum class LateralControlMode
	{
		NoRequest = 0, // Not requesting lateral control / 不请求横向控制
		Position = 1, // Control steer, value ranges from -1(max right turning) ~ 1(max left turning) / 控制方向盘，数值范围为-1(右打到底)~1(左打到底)
		Curvature = 2, // Control turning curvature, in 1/m units / 控制转弯曲率，数值单位为1/m
	};

	enum class GearControlMode
	{
		NoRequest = 0, // Not requesting gear switching / 不请求换挡
		Drive = 1, // Switch to drive gear / 请求换D挡
		Reverse = 2, // Switch to reverse gear / 请求换R挡
	};

	struct VehicleControlData
	{
		LongitudinalControlMode longitudinalMode; // 纵向控制模式
		Double longitudinalValue; // 纵向控制数值
		Double targetSpeed; // [KPH] Target absolute speed, only available on acceleration control mode / 目标绝对速度，仅限加速度控制模式下有效

		LateralControlMode lateralMode; // 横向控制模式
		Double lateralValue; // 横向控制数值

		GearControlMode gearMode; // 换挡控制模式

		VehicleControlData()
		{
			longitudinalMode = LongitudinalControlMode::NoRequest;
			longitudinalValue = 0;
			targetSpeed = 0;

			lateralMode = LateralControlMode::NoRequest;
			lateralValue = 0;

			gearMode = GearControlMode::NoRequest;
		}
	};

	class VehicleControl : public SessionSample
	{
	private:
		class VehicleControlVars : public SessionSampleVars
		{
		public:
			SPADAS_VARS(VehicleControl, SessionSampleVars)
			VehicleControlData data;

			SessionGeneralSample toGeneralSample()
			{
				SessionGeneralSample sample;
				sample.timestamp() = timestamp;

				Array<GeneralElement> values(6);
				sample.values() = values;

				values[0] = (Double)(Int)data.longitudinalMode;
				values[1] = data.longitudinalValue;
				values[2] = data.targetSpeed;
				values[3] = (Double)(Int)data.lateralMode;
				values[4] = data.lateralValue;
				values[5] = (Double)(Int)data.gearMode;

				sample.significantCount() = values.size();
				return sample;
			}

			Bool fromSample(String protocol, SessionSample sample) override
			{
				SessionGeneralSample generalSample;
				if (!sample.is<SessionGeneralSample>(generalSample)) return FALSE;

				timestamp = generalSample.timestamp();
				auto values = generalSample.values();

				if (protocol == "vehicle-control-v1")
				{
					if (values.size() != 6) return FALSE;

					data.longitudinalMode = values[0].valid ? (LongitudinalControlMode)(Int)values[0].value : LongitudinalControlMode::NoRequest;
					data.longitudinalValue = values[1].value;
					data.targetSpeed = values[2].value;
					data.lateralMode = values[3].valid ? (LateralControlMode)(Int)values[3].value : LateralControlMode::NoRequest;
					data.lateralValue = values[4].value;
					data.gearMode = values[5].valid ? (GearControlMode)(Int)values[5].value : GearControlMode::NoRequest;

					return TRUE;
				}

				return FALSE;
			}
		};

	public:
		SPADAS_CLASS("vehicle_control_v1.VehicleControl", VehicleControlVars)

		VehicleControl() : SessionSample(new VehicleControlVars)
		{}
		VehicleControlData *operator ->()
		{
			return &var()->data;
		}

		static String getProtocolName(UInt channel)
		{
			return (String)"vehicle-control-v1@" + channel;
		}
		SessionGeneralSample toGeneralSample()
		{
			return var()->toGeneralSample();
		}

		static Array<String> getProtocolNames(UInt channel)
		{
			return Array<String>::create(1, "vehicle-control-v1").convert<String>([channel](auto& s){ return s + "@" + channel; });
		}
		Bool fromGeneralSample(String protocol, SessionGeneralSample generalSample)
		{
			return fromSample(protocol, generalSample);
		}
	};
}

#endif
